// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; version 2 of the License.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

#include "Orbit.h"
#include "GravBody.h"
#include "Object.h"
#include "physics.h"
//#include "OrbitSolver.h"
#include <vector>
//#define WIN32_LEAN_AND_MEAN
//#define VC_EXTRALEAN
//#include "windows.h"

//std::vector<PhysBody*> *bodies;
//std::vector<PhysObject*> objects;

//HANDLE objects_mutex;
//HANDLE exit_mutex;

/* RK6 constants */
double b21=1./5.;
double b31=3./40., b32=9./40.;
double b41=3./10., b42=-9./10., b43=6./5.;
double b51=-11./54., b52=5./2., b53=-70./27., b54=35./27.;
double b61=1631./55296., b62=175./512., b63=575./13824., b64=44275./110592., b65=253./4096.;

/*
DWORD WINAPI physicsMain(LPVOID _bodies)
{	
	objects_mutex = CreateMutex(NULL, FALSE, NULL);
	exit_mutex = CreateMutex(NULL, FALSE, NULL);
	bodies = (std::vector<PhysBody*> *)_bodies;
	std::vector<PhysBody*>::iterator biter;
	std::vector<PhysObject*>::iterator oiter;
	Ogre::Timer* timer = new Ogre::Timer();
	double jd = 2454638.82294;
	double dt = 0.;
	unsigned long lastt;
	unsigned long newt;
	std::cout.setf(std::ios::fixed, std::ios::floatfield);
	lastt = timer->getMicroseconds();
	while (1) {
		newt = timer->getMicroseconds();
		dt = ((newt - lastt) / 1000000.) * 1000000;
		lastt = newt;
		jd += dt/(3600.0 * 24.0);
		for( biter = bodies->begin(); biter != bodies->end(); biter++ ) {
			(*biter)->update(jd);
		}
		WaitForSingleObject(objects_mutex, INFINITE);
		for (oiter = objects.begin(); oiter != objects.end(); oiter++ ) {
			(*oiter)->update(dt, bodies);
		}
		ReleaseMutex(objects_mutex);

		WaitForSingleObject(exit_mutex, INFINITE);
		if (shutdown) {
			ReleaseMutex(exit_mutex);
			break;
		}
		ReleaseMutex(exit_mutex);

		Sleep(2);
		//sched_yield();
	}
	//bodies->clear();
	objects.clear();
	CloseHandle(objects_mutex);
	CloseHandle(exit_mutex);
	return 0;
}

void addObject(PhysObject* object)
{
	WaitForSingleObject(objects_mutex, INFINITE);
	objects.push_back(object);
	ReleaseMutex(objects_mutex);
}
*/

VECTOR gravity_acceleration( VECTOR &affected_body_position,
	const VECTOR &gravitating_body_position, double gravitating_body_mass )
{
	VECTOR acc_direction;
	acc_direction=gravitating_body_position - affected_body_position;
	double distance = acc_direction.length();
	acc_direction.normalize();
	double acc =( gravitating_body_mass * G ) / ( distance * distance );
	return (acc_direction * acc);
}

VECTOR rk6_gravity_acceleration( double delta_t, VECTOR &affected_body_position,
	const VECTOR &gravitating_body_position,double gravitating_body_mass)
{
	VECTOR kx1,kx2,kx3,kx4,kx5,kv1;
	kx1 = gravity_acceleration(affected_body_position,gravitating_body_position, gravitating_body_mass)*delta_t;
	kv1 = affected_body_position + ( kx1 * b21 );
	kx2 = gravity_acceleration( kv1,gravitating_body_position, gravitating_body_mass)*delta_t;
	kv1 = affected_body_position + ( kx1 * b31 ) + ( kx2 * b32 );
	kx3 = gravity_acceleration( kv1,gravitating_body_position, gravitating_body_mass)*delta_t;
	kv1 = affected_body_position + ( kx1 * b41 ) + ( kx2 * b42 ) + ( kx3 * b43 );
	kx4 = gravity_acceleration( kv1,gravitating_body_position, gravitating_body_mass)*delta_t;
	kv1 = affected_body_position + ( kx1 * b51 ) + ( kx2 * b52 ) + ( kx3 * b53 ) + ( kx4 * b54 );
	kx5 = gravity_acceleration( kv1,gravitating_body_position, gravitating_body_mass)*delta_t;
	kv1 = affected_body_position + ( kx1 * b61 ) + ( kx2 * b62 ) + ( kx3 * b63 ) + ( kx4 * b64 ) + ( kx5 * b65 );
	return gravity_acceleration( kv1,gravitating_body_position, gravitating_body_mass)*delta_t;
}
